import io
from picamera import PiCamera
import time
import numpy as np
import cv2
import imgprocess
import pwm
import search
import color

norm_speed=45
camera = PiCamera()
camera.resolution = (640, 480)
car=pwm.CarMove()
height = 380
width = 640
stream = io.BytesIO()
cnt=0
count=0
historyangle = 0
for frame in camera.capture_continuous(stream, format='jpeg', use_video_port=True):
    cnt=(cnt+1)%10
    count+=1
    data = np.fromstring(stream.getvalue(), dtype=np.uint8)
    img = cv2.imdecode(data,cv2.IMREAD_COLOR)
    cv2.namedWindow('image', cv2.WINDOW_NORMAL)
    frame,center=color.color(img)
    if len(center)>0:
        centery=center[0]
        angle=color.decide(width, centery)
        print("转向角度",angle)
        cv2.circle(frame, center, 5, (0, 0, 255), -1)
        cv2.line(frame, (center[1],0), (center[1], 100), (0, 255, 0),2)
        cv2.imshow('image', frame)
        cv2.waitKey(0)
        cv2.destroyAllWindows()
    else:angle=-180
    # cv2.imwrite("img/img"+str(count)+"转向角度："+str(angle)+".jpg",img)
    # cv2.imwrite("static/videos/video"+str(cnt)+".jpg",img)
    pwm.control(norm_speed,angle,car)
    stream.truncate()
    stream.seek(0)


